﻿using PropertyChanged;
using System;
using System.Collections.Generic;
using System.Drawing.Imaging;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace VIA.SiMoJi.Domain.Vision
{
    [AddINotifyPropertyChangedInterface]
    public class Pose
    {
        public double X { get; set; }
        public double Y { get; set; }
        /// <summary>
        /// 角度单位：度
        /// </summary>
        public double Angle { get; set; }

        //构造函数
        public Pose()
        {
        }
        public Pose(double x, double y, double angle)
        {
            X = x;
            Y = y;
            Angle = angle;
        }

        public void ReverseDirection()
        {
            double a = Angle % 360;
            if (a >= 180 && a <= 360)
            {
                a -= 360;
            }
            else if (a <= -180 && a >= -360)
            {
                a += 360;
            }
            if (a >= 0)
                a -= 180;
            else
                a += 180;
        }

        //Pose转矩阵
        public CMatrix ToMatrix()
        {
            CMatrix colMatrix = new();
            colMatrix.Rotate(Angle);
            colMatrix.Translate(X, Y);
            return colMatrix;
        }

        public override string ToString()
        {
            return $"X:{X:0.###}, Y:{Y:0.###}, Angle:{Angle:0.###}";
        }

        public static Pose operator +(Pose p1, Pose p2)
        {
            return new Pose(p1.X + p2.X, p1.Y + p2.Y, p1.Angle + p2.Angle);
        }

        public static Pose operator -(Pose p1, Pose p2)
        {
            return new Pose(p1.X - p2.X, p1.Y - p2.Y, p1.Angle - p2.Angle);
        }

        public Pose Rotate(double angle)
        {
            var mat = this.ToMatrix();
            mat.Rotate(angle);
            return mat.ToPose();
        }
    }
}
